#!/usr/bin/env python
# -*- coding: utf-8 -*-
import rospy
import cv2
import numpy as np
from cv_bridge import CvBridge, CvBridgeError
from sensor_msgs.msg import Image
from geometry_msgs.msg import PoseStamped



def empty(a):
    pass

#定义一个寻找边缘的函数
def getContours(img,imgContour,imgColor,mask):

    a,contours,hierarchy = cv2.findContours(img,cv2.RETR_EXTERNAL,cv2.CHAIN_APPROX_NONE)#寻找轮廓，第二个参数为模式，第三个为近似方法
    for cnt in contours:
        area = cv2.contourArea(cnt)
        #print(area)
        if area>250:                                        #通过面积来筛选符合要求的轮廓范围，减少噪声的干扰
            cv2.drawContours(imgContour,cnt,-1,(0,255,0),3) #画出边界
            peri = cv2.arcLength(cnt,True)                  #计算弧长（周长）
            #print(peri)
            approx = cv2.approxPolyDP(cnt,0.02*peri,True)   #得到角点
            #print(len(approx))                              #通过角点的个数来判断几何图形是几边形
            objCor = len(approx)
            x,y,w,h = cv2.boundingRect(approx)              #通过角点得到外接矩形
            if mask[y+h//2,x+w//2]==255:                              # 先判断边缘中心是否在mask之内
                cv2.drawContours(imgContour, cnt, -1, (0, 255, 0), 3)   # 画出边界
                if objCor == 3: objectType = "Tri"                      #判断图形边数
                elif objCor == 4:
                    aspRatio = w/float(h)                               #计算得到长宽比
                    if aspRatio > 0.95 and aspRatio <1.05:
                        objectType = "Square"                           #长宽比范围在1附近识别为正方形
                    else:objectType = "Rectangle"                       #其余识别为长方形
                elif objCor > 4: objectType = "Circle"
                else: objectType = "None"
                cv2.rectangle(imgColor,(x,y),(x+w,y+h),(0,255,0),2) #画出外接矩形
                
                cv2.putText(imgColor,objectType,                      #标出形状
                            (x+(w//2)-10,y+(h//2)-10),cv2.FONT_HERSHEY_COMPLEX,0.5,
                            (0,0,0),2)
                return x+(w//2),y+(h//2)
                            
                cv2.drawMarker(imgContour,(x+w//2,y+h//2),(0,0,0),5,5)
            
    return -1,-1
"""
cv2.namedWindow("TrackBars")
cv2.resizeWindow("TrackBars",640,330)
cv2.createTrackbar("Hue Min","TrackBars",0,255,empty)
cv2.createTrackbar("Hue Max","TrackBars",255,255,empty)
cv2.createTrackbar("Sat Min","TrackBars",0,255,empty)
cv2.createTrackbar("Sat Max","TrackBars",255,255,empty)
cv2.createTrackbar("Val Min","TrackBars",0,255,empty)
cv2.createTrackbar("Val Max","TrackBars",255,255,empty)
"""

class image_converter:
    def __init__(self):    
        # 创建cv_bridge，声明图像的发布者和订阅者
        self.massage_pub = rospy.Publisher("talker",PoseStamped, queue_size=1)
        self.bridge = CvBridge()
        self.image_sub = rospy.Subscriber("/camera/color/image_raw", Image, self.callback)
        #self.image_sub = rospy.Subscriber("/iris/usb_cam/image_raw", Image, self.callback)

    def callback(self,data):
        # 使用cv_bridge将ROS的图像数据转换成OpenCV的图像格式
        try:
            cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
            cv_image = cv2.resize(cv_image, (640, 480))
        except CvBridgeError as e:
            print (e)
	    #cv2.imshow("original",cv_image)
        imgContour = cv_image.copy()
	
        imgGray = cv2.cvtColor(cv_image,cv2.COLOR_BGR2GRAY)
        imgHSV  = cv2.cvtColor(cv_image,cv2.COLOR_BGR2HSV)
        """
            h_min = cv2.getTrackbarPos("Hue Min","TrackBars")
            h_max = cv2.getTrackbarPos("Hue Max","TrackBars")
            s_min = cv2.getTrackbarPos("Sat Min","TrackBars")
            s_max = cv2.getTrackbarPos("Sat Max","TrackBars")
            v_min = cv2.getTrackbarPos("Val Min","TrackBars")
            v_max = cv2.getTrackbarPos("Val Max","TrackBars")
        """        
        #lower = np.array([h_min,s_min,v_min])
            #upper = np.array([h_max,s_max,v_max])

        #red
        #lower = np.array([160,120,180])
        #upper = np.array([180,220,255])
	#red-3.4
        lower = np.array([160,70,170])
        upper = np.array([180,230,255])
        #blue
        #lower = np.array([90,210,190])
        #upper = np.array([110,255,250])

        mask = cv2.inRange(imgHSV,lower,upper)
        mask = cv2.dilate(mask, np.ones((3, 3), np.uint8))
        mask = cv2.erode(mask,np.ones((3,3),np.uint8))

        imgColor = cv2.bitwise_and(cv_image,cv_image,mask=mask)
        imgGray = cv2.cvtColor(imgColor,cv2.COLOR_BGR2GRAY)
        imgBlur = cv2.GaussianBlur(imgGray,(7,7),1)
        imgCanny = cv2.Canny(imgBlur,15,40)
        imgCanny = cv2.dilate(imgCanny, np.ones((3, 3), np.uint8))
        imgCanny = cv2.erode(imgCanny, np.ones((3, 3), np.uint8))
        
        res=getContours(imgCanny,imgContour,imgColor,mask)
	
        #imgBlank = np.zeros_like(cv_image)	

        #imgStack =  stackImages(0.8,([cv_image,imgGray,imgBlur],
        #                            [imgCanny,imgContour,imgBlank]))
        
        #cv2.imshow("imgContour",imgContour)
        cv2.imshow("imgColor",imgColor)
        cv2.waitKey(1)
        if(res[0]!=-1):
            print(res)
        # 再将opencv格式额数据转换成ros image格式的数据发布
        try:
	    #mypose=PoseStamped()
	    #self.massage_pub.publish(mypose) #先发送一个空位置，试探一下，否则第一个包容易丢
	    #time.sleep(1)
	    
            mypose=PoseStamped()
            mypose.header.frame_id='target' #设置自己的目标
            mypose.pose.position.x=res[0]
            mypose.pose.position.y=res[1]
            mypose.pose.position.z=0
            mypose.pose.orientation.x=0
            mypose.pose.orientation.y=0
            mypose.pose.orientation.z=0
            mypose.pose.orientation.w=1
            self.massage_pub.publish(mypose)
        except CvBridgeError as e:
            print (e)

if __name__ == '__main__':
    try:
        # 初始化ros节点
        rospy.init_node("cv_bridge_test")
        rospy.loginfo("Starting cv_bridge_test node")
        image_converter()
        rospy.spin()
    except KeyboardInterrupt:
        print ("Shutting down cv_bridge_test node.")
        cv2.destroyAllWindows()
